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ABSTRACT 

This  work  describes  the  designing  ofAHR  System  with  TGIC  (Two  Step  Geometrical  Intuitive  Correction )  algorithm  using 
Kalman  Filter  to  provide  a  proper  orientation.  A  multi  sensor  IMU  is  required  because  a  single  type  sensor  cannot  provide 
accurate  attitude  estimation.  The  accel  erometer  is  sensitive  to  vibrations.  A  gyroscope  is  susceptible  to  low-frequency  drift 
and  wideband  measurement  noise,  resulting  in  accumulation  of  bias,  and  the  magnetometer  readings  get  distorted  due  to 
the  ferrous  materials  ( magnetic  material ).  Therefore,  it  is  passed  through  a  correction  algorithm  to  reduce  these 
distortions.  TGIC  with  Kalman  Filter  helps  in  reducing  these  distortions.  AHRS  application  includes  Drones  (UAVs), 
navigation  tracking  etc.  Kalman  filter  is  an  estimation  algorithm.  It's  a  method  of  calculating  the  future  state  of  a  system 
based  on  the  prior  state.  We  are  using  a  quaternion  based  Extended  Kalman  filter,  where  the  quaternion  with  the  gyro  bias 
represents  the  state  vector.  Including  the  bias  that  is  in  the  state  vector  gives  ability  to  track  the  bias  and  model  the  drift  in 
Kalman  filter  to  reduce  error. 
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INTRODUCTION 

An  attitude  and  heading  reference  system  (AHRS)  is  made  up  of  sensors  on  three  vertices  that  give  attitude  info  for  the 
aircraft,  i.e.  roll,  pitch  and  yaw.  These  are  referred  to  as  MARG  (Magnetic,  Angular  Rate,  and  Gravity)  sensors  and  are  of 
either  solid  or  micro  electro  mechanical  system  known  as  (MEMS)  consisting  gyroscopes,  accelerometers  and 
magnetometers.  They  are  designed  to  replace  traditional  mechanical  gyroscopic  flight  instruments. 

[1]  A  type  of  non-linear  estimation  system  such  as  Ex-tended  Kalman  filter  is  usually  used  to  compute  the  result 
from  these  sources.  AHRS  have  been  proven  to  be  highly  dependable  and  are  in  common  use  in  all  types  of  aircrafts.  AHRS 
are  typically  paired  with  electronic  flight  instrument  systems  (EFIS)  to  form  the  primary  flight  display.  AHRS  are  combined 
with  air  data  computers  to  create  an  “air  data,  attitude  and  heading  reference  system”  (ADAHRS),  which  provides 
information  like  air-speed,  altitude  and  air  temperature.  With  sensor  fusion,  drift  from  gyroscopes  integration  is  made  up  for 
by  the  reference  vectors,  gravity,  and  the  earth’s  magnetic  field.  This  results  in  a  drift-less  orientation,  making  an  AHRS  a 
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more  cost  operative  solution  than  traditional  IMUs  (Inertial  Measurement  Units)  that  only  integrate  gyroscopes  and  work  on 
a  high  bias  stability  of  the  gyroscopes. 

AHRS  Model 

Figure  1  Air  traffic  control  -  standard  international  practice  is  to  monitor  airspace  using  two  radar  systems:  primary  and 
secondary. 

Primary  radar  -based  on  the  earliest  form  of  radar  developed  in  the  1930s,  detects  and  measures  the  approximate 
position  of  aircraft  using  reflected  radio  signals. 

Secondary  radar  -  which  relies  on  targets  being  equipped  with  a  transponder,  also  requests  additional  information 
from  the  aircraft  -  such  as  its  identity  and  altitude. 

All  commercial  aircraft  are  equipped  with  transponders  (an  abbreviation  of  "transmitter  responder"),  which 
automatically  transmit  a  unique  four-digit  code  when  they  receive  a  radio  signal  sent  by  radar. 

x 

GPS:  Shows  pilots  their  position  / 

but  not  normally  used  by  ATC  » 


Primary  radar:  Can  only  show  approx  position  No  radar  coverage  240km  from  land 


Figure  1:  Radar  Based  Flight  Tracking  System. 

The  code  gives  the  plane's  identity  and  radar  stations  go  on  to  establish  speed  and  direction  by  monitoring 
successive  transmissions.  This  flight  data  is  then  relayed  to  air  traffic  controllers.  However,  once  an  aircraft  is  more  than 
240km  (150  miles)  out  to  sea,  radar  coverage  fades  and  air  crew  keep  in  touch  with  air  traffic  control  and  other  aircraft  using 
high-frequency  radio. 

To  reduce  the  computational  time,  and  improve  the  pitch/roll  approximation  accuracy  of  the  low-cost  attitude 
heading  reference  system  in  conditions  of  magnetic-distortions,  an  innovative  linear  Kalman  filter,  apt  for  nonlinear  attitude 
estimation,  is  projected  in  this  paper.  The  novel  algorithm  is  the  amalgamation  of  two-step  geometrically-intuitive  correction 
and  the  Kalman  filter[2]. 

Two-step  system  is  used  to  make  the  present  approximation  of  pitch/roll  resistant  to  magnetic  distortions.  The 
TGIC  outputs  a  calculated  quaternion  for  the  Kalman  filter,  which  evades  the  linearization  error  of  measurement  equations 
and  decreases  the  computational  time. 

Figure  2  is  the  block  diagram  of  proposed  AHRS  system;  Figure3  shows  orientation  across  axes.  As  Accelerometer 
is  sensitive  to  vibrations.  ADXL345  is  a  3-axis  accelerometer  with  high  resolution  (13-bit)  measurement  at  up  to  ±16  g,  its 
value  are  passed  through  a  low-pass  filter.  Mag  (m)  values  from  magnetometer  HMC5883L  a  surface-mount,  multi-chip 
module  designed  for  low-field  magnetic  sensing  with  a  digital  interface  and  the  BMP-085  is  digital  pressure  sensor  which 
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has  high  accuracy  and  long  term  stability.  And  low  passed  accelerometer  values  are  then  given  to  TGIC  to  correct  the 
estimated  direction  of  gravity  and  convert  to  the  quaternions  (q).  These  quaternions  and  gyro  values  from  gyroscope 
L3G400D  a  low-power  three-axis  angular  rate  sensor  are  then  passed  through  the  Kalman  filter.  Kalman  filter  keeps  the 
track  of  the  bias  and  helps  in  removing  the  error  through  prediction  and  updation  step  which  gives  the  corrected  value  of  roll 
(T),  pitch(0)  and  yaw(\|/). 
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Figure  2:  Block  Diagram  of  Proposed  AHRS  System. 
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Figure  3:  Orientation  across  the  Axes. 

To  detail  the  orientation,  set  the  body  frame  a  xyz,  the  navigation  frame  m  North,  Down,  East  (NDE).  The  y-axis  is 
oriented  towards  the  forward,  the  x-axis  is  oriented  towards  the  down  and  the  z-axis  is  oriented  towards  the  right  [2].  The 
angle  of  the  body  could  be  gotten  from  an  attitude  transformation  matrixC^.  is  a  perpendicular  matrix  and  can  be 
summed  through  3  separate  rotations  around  the  3  axes.  The  first  rotation  is  along  the  y-axis  by,  the  second  revolution  is 
around  the  z-axis  by,  and  the  third  spin  is  around  the  x-axis  by;  they  are  given  as: 
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Because  of  the  disadvantages  of  the  Euler  angle,  quaternion  a ™  =  [pO  pi  p2  p3]  is  utilized  to  mean  the  attitude  of 
the  m  frame  in  reference  to  a  frame.  Therefore,  the  direction  cosine  matrix  (DCM)  can  be  represented  in  quaternion: 


c*  -- 


qO2  +  q  l2  -  q22  -  q32 
-2q0q3  +  2qlq2 
2q0q2  +  2qlq3 


2qQq3  +  2qlq2 
qO2  -  ql2  +  q22  -  q32 
—2q0ql  +  2q2q3 


-2q0q2  +  2qlq3 
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(3) 


The  attitude  angles  can  be  calculated  as: 
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V  =  ac  tan((2gOg2  -  2qlq3)/(q02  +  ql2  -  q22  -  q32)) 

0=  ac  sin(2q0<?3  +  2qlq2) 

y=  ac  tan((2qrOgl  -  2q2q3)/(q02  -  ql2  +  q22  -  q32)) 

(4) 


From  accelerometer  the  roll  and  pitch  are  calculated  by: 


fx 
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(5) 

Where  fx,  fy  and  fz  represent  the  measures  of  the  accelerometer  in  the  a  frame;  and  g  stands  for  the  gravitational 
acceleration.  Then,  the  yaw  and  roll  is: 
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Similarly,  for  pitch: 
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Where  Hb  is  the  earth  magnetic  field  in  the  a  frame  and  H1  in  horizontal  Frame.  Thus,  the  pitch  is  given  by: 


(8) 


Where  D  is  the  distortion  due  to  the  magnetic  components  around  the  device.  The  calculated  vector  of  gravitational 
field  and  electromagnetic  field  are  obtained  by: 


r  §b  =  C>tiq)gn 
1flb  =  C%(bq)mn 
9n=[  0  1  Of 
jnn  =  [m,v  mu  0]r 


(9) 


g b  =  [*X  ay  az]T  and  mb  -  pn,  my  m2y 

An  AHRS  system  includes  two  separate  functioning  blocks.  They  are: 

Two-Step  Geometrically-Intuitive  Correction 

Figure  4  A  TGIC  the  step  wise  two-step  geometrically-intuitive  corrective  system  is  the  method  used  to  make  the  present 
approximation  of  pitch/roll  invulnerable  to  magnetic  distortions.  It  gives  a  calculated  quaternion  value  for  the  Kalman  filter, 
which  does  not  give  the  linearization  error  of  measurement  equivalences  and  decreases  the  computational  time. 
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Figure  4:  Block  Diagram  of  TGIC’s  1st  Step. 


Quaternions  obtained  from  the  Euler  angles  are  passed  to  TGIC  first  step  correction  where  the  correction  of 
projected  track  of  gravity  is  done  as  shown  in  the  Figure  5,  correction  for  the  assessed  direction  of  gravity  is  done  by 
spinning  the  least  valued  attitude  quaternion  qk  by  the  angle  b  (the  angle  in  vg  &  vg)  about  the  axis  na  (that  is  defined  by  the 
dot  multiplication  of  vg  &  vg).  Therefore,  the  matching  error  quaternion  qae  and  calculated  orientation  qa  can  be  given  by: 

{Va&Qn\  ,  ■+  . 

qae  =  cos  1^— —  J  +  na  Sill  (  ^  J 


=  <?=.*  &  <7 jt 


(12) 


Where, 


na  =  Vg  X  $g,  Ada  =  fl  cos(vg  ■  V3) 


(13) 


The  variable  ‘c’  is  utilized  to  decrease  the  effect  of  the  exterior  acceleration.  By  partly  modifying  the  angle  c,  the 
influence  of  outside  acceleration  will  be  found  to  be  near  to  0.  The  prime  choice  for  ‘c’  is  so  that  the  system  can  produce  a 
strong  attitude  in  static  and  active  tests,  not  overshooting.  The  calculation  of  the  variable  ‘c’  in  many  different  situations  will 
be  shown  in  the  experimentation  section. 


Figure  5:  Correcting  the  Projected  Direction  of  Gravity 
using  the  Accelerometer. 


Ex- tended  Kalmanian  Filter 
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Under  estimation  theory,  the  ex-tended  Kalmanian  filter  (EKF)  is  a  non-linear  form  of  the  Kalmanian  filter  that  linearizes 
around  an  approximation  of  the  present  average  and  covariance.  In  the  conditions  of  a  well-  structured  transition  model,  the 
EKF  has  been  regarded  as  standard  in  the  field  of  nonlinear  value  estimation,  navigating  systems  and  GPS  system.  [6] 
Accelerometer,  gyro  and  mag  values  are  taken  from  IMU  GY80.  TGIC  consists  of  two  steps  to  be  folio wed.First  step  only 
includes  the  correction  of  accelerometer  values  and  second  step  includes  the  correction  of  magnetometer  values.  The 
combined  steps  gives  the  calibrated  roll,  pitch  and  yaw  values.  The  values  are  passed  to  the  Extended  Kalman  filter  in  the 
form  of  quaternions  where  prediction  and  updating  step  are  used  to  reduce  the  error.  [3]  The  calculation  of  the  theorized 
Kalmanian  filter  starts  with  in  initial  conditions: 


=  ERd 

P0  =  E[(X0-Xe)(X0-Xay] 


(14) 


The  original  covariance  matrix  Po  is  permanently  given  a  big  positive  number  in  order  to  attain  a  steady  filter  and  it 
is  calculated  that  Po  =  10- 1  [3x3].  I  [3x3]  is  the  4x4  identity  matrix.  The  step  next  is  to  show  the  current  vale  and  covariance 
approximations  from  time  stamp  kl  to  step  k2: 


Where  exp('QkT)  is  the  separate  time  stamp  transition  matrix;  &  Qk  is  the  process  errorcovariance  associated  to  the 
quaternion.  Then,  the  Kalman  gain  is  calculated  as: 


Rtr+i  —  exP  (®-k&T)Xk 
A"+1  =  e^p(O^AT’)  Pk  exp(ilfeA T)r  +  Qk 


(15) 


where  Rk+i  is  the  measurement  noise  covariance.  The  final  thing  to  do  is  to  calculate  the  post  error  covariance 
approximate: 


Rk+i  -  p*+ i(As+i  +  ^4+i) 

(16) 

where  Zk+2  is  the  calculated  quaternion.  From  the  steps  of  the  Kalmanian  filter  detailed  above,  we  can  attain  the 
prime  estimated  quaternion  and  finally  obtain  the  3-D  attitude  of  the  aircraft.  The  quaternions  from  the  second  step  i.e  Xk+1  is 
used  to  obtain  the  attitude  (roll,  pitch  and  yaw)  as  in  equation  (3). 

AHRS  Algorithm 

The  proposed  AHRS  in  this  paper  is  a  predict  and  updating  technique  where  we  detect  the  error  using  Kalman  Filter  and 
estimating  correct  direction  gravity  using  TGIC  as  mentioned  in  Figure5.  First,  we  allow  the  calibration  of  the  pitch,  roll 
and  acceleration  of  the  IMU  to  take  place  for  5  seconds.  We  calculate  the  mean  pitch  angle,  mean  roll  angle,  mean  yaw 
angle  and  mean  magnetometer  reading  x,  y,  z  directions.  This  mean  pitch,  roll  and  acceleration  calculated  are  used  as  a 
reference  for  setting  thresholds  and  to  form  a  quaternion. 

The  sender  and  Receiver  part  of  the  procedure  shown  in  Figure  6  as  follows: 
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Figure  6:  Flowchart  of  Sending  and  Receiving  Data  from 
Controller  to  Device. 

Working  of  Extended  kalman  filter  with  TGIC  is  shown  in  the  flowchart  Figure7. 
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Figure  7:  Flowchart  of  Kalman  Filter  with  TGIC. 

EXPERIMENT AION  AND  RESULT 


Figure  8  shows  the  circuit  set  up.  We  first  collect  the  data  from  the  accelerometer,  gyroscope  and  magnetometer  of  the  GY- 
80.  We  first  conduct  an  experiment  where  the  IMU  is  placed  stationary  on  a  flat  table  and  we  get  the  raw  roll,  pitch,  and 
yaw  at  a  sample  rate  of  100Hz.  We  take  the  first  500  samples  as  calibration  time  and  then  the  above  data  is  passed  through 
the  algorithm  and  the  result  is  shown.  The  plot  of  raw  roll,  pitch  and  yaw  wise  rotated  data  are  shown  in  the  following 
figures. 
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Figure  8:  Circuit  Diagram  with  Model. 
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Figure  9:  Plot  of  Raw  PITCH  wise  Rotated  Data  (Degree). 
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Figure  10:  Plot  of  Estimated  PITCH  wise  Data(Degree). 
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Figure  11:  Plot  of  Raw  ROLL  wise  Rotated  Data  (Degree). 
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Figure  12:  Plot  of  Estimated  ROLL  wise  Data(Degree). 
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Figure  13:  Plot  of  Raw  YAW  wise  Rotated  Data  (Degree). 


Figure  14:  Plot  of  Estimated  YAW  wise  Data(Degree). 
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Figure  15:  Plot  of  Raw  PITCH,  ROLL  and  YAW  wise  Rotated  Data  (Degree). 
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Figure  16:  Plot  of  Estimated  Pitch, Roll  and  Yaw  wise  Rotated  Data  Lot  of  Estimated  Data(Degree). 
CONCLUSIONS 

In  this  paper,  Extended  Kalman  filter  was  pro-posed  for  the  orientation  estimation  of  AHRS.  The  Kalman  filter  was 
significantly  simplified  by  preprocessing  the  accelerometer  and  magnetometer  information  using  a  two-step  geometrically- 
intuitive  correction  (TGIC)  approach.  Compared  with  the  traditional  external  quaternion  estimation  algorithm,  the  use  of 
two-step  correction  decouples  the  accelerometer  and  magnetometer  information.  Here  we  used  only  first  step  later  with  mag 
also  can  be  implemented.  This  decoupling  eliminates  the  influence  of  magnetic  interference  on  the  current  estimation  of 
pitch/roll.  In  addition,  the  quaternion  produced  by  the  TGIC  is  utilized  as  the  input  observation  vector  for  the  Kalman  filter, 
which  avoids  the  linearization  error  of  measurement  equations  and  reduces  the  computational  complexity  of  the  filter. 
By  carrying  out  several  experiments,  the  performance  of  the  proposed  filter  in  static  and  dynamic  conditions  was  verified. 
The  experimental  results  indicate  that  the  proposed  Kalman  filter  is  able  to  provide  relatively  faster  and  more  accurate  real¬ 
time  orientation  information  in  different  working  conditions. 
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